#pragma once

#include "zuc/nml_intf/zuc.hh"
#include "libnml/nml/cmd_msg.hh"
#include "zuc/nml_intf/zucpos2.h"

#define DEC_CMD_HEADER(_NAME)                                \
    _NAME() : ZUC_ROB_CMDITF(_NAME##_TYPE, sizeof(_NAME)) {} \
    virtual void update_param(CMS* cms) override;            \
    virtual const char* msg_name() override { return #_NAME; }

#define DEC_CMD_P0(_NAME)                                          \
    class _NAME : public ZUC_ROB_CMDITF                            \
    {                                                              \
    public:                                                        \
        _NAME() : ZUC_ROB_CMDITF(_NAME##_TYPE, sizeof(_NAME)) {}   \
        virtual void update_param(CMS* cms) override {}            \
        virtual const char* msg_name() override { return #_NAME; } \
    }

#define DEC_CMD_PN(_NAME, V)            \
    class _NAME : public ZUC_ROB_CMDITF \
    {                                   \
    public:                             \
        DEC_CMD_HEADER(_NAME)           \
        V;                              \
    }

/****************************** ROB CMD *******************************/

class ZUC_ROB_CMDITF : public RCS_CMD_MSG
{
public:
    ZUC_ROB_CMDITF(NMLTYPE t, size_t s) : RCS_CMD_MSG(t, s) { robot_id = 0; }
    virtual void update(CMS* cms) final;
    virtual void update_param(CMS* cms) = 0;

    uint8_t robot_id;  //多机器人时，标识机器人的id
};

/*
 * ZUC_JOG_CMD_MSG class.
 */
class ZUC_JOG_CMD_MSG : public ZUC_ROB_CMDITF
{
public:
    ZUC_JOG_CMD_MSG(NMLTYPE t, size_t s) : ZUC_ROB_CMDITF(t, s){};

    // For internal NML/CMS use only.
    void update_param(CMS* cms);

    // joint_or_axis == joint_number          for joint jogs (jjogmode==1)
    // joint_or_axis == 0 for X, 1 for Y,...  for axis  jogs (jjogmode==0)
    int joint_or_axis;
};
class ZUC_JOG_CONT : public ZUC_JOG_CMD_MSG
{
public:
    ZUC_JOG_CONT() : ZUC_JOG_CMD_MSG(ZUC_JOG_CONT_TYPE, sizeof(ZUC_JOG_CONT)){};
    void update_param(CMS* cms);
    virtual const char* msg_name() override { return "ZUC_JOG_CONT"; }
    int jjogmode;
    double vel;
};
class ZUC_JOG_INCR : public ZUC_JOG_CMD_MSG
{
public:
    ZUC_JOG_INCR() : ZUC_JOG_CMD_MSG(ZUC_JOG_INCR_TYPE, sizeof(ZUC_JOG_INCR)){};
    void update_param(CMS* cms);
    virtual const char* msg_name() override { return "ZUC_JOG_INCR"; }
    double incr;
    int jjogmode;
    double vel;
};
class ZUC_JOG_ABS : public ZUC_JOG_CMD_MSG
{
public:
    ZUC_JOG_ABS() : ZUC_JOG_CMD_MSG(ZUC_JOG_ABS_TYPE, sizeof(ZUC_JOG_ABS)){};
    void update_param(CMS* cms);
    virtual const char* msg_name() override { return "ZUC_JOG_ABS"; }
    double pos;
    int jjogmode;
    double vel;
};

class ZUC_JOG_MULTI_JOINT : public ZUC_JOG_CMD_MSG
{
public:
public:
    ZUC_JOG_MULTI_JOINT() : ZUC_JOG_CMD_MSG(ZUC_JOG_MULTI_JOINT_TYPE, sizeof(ZUC_JOG_MULTI_JOINT)){};
    void update_param(CMS* cms);
    virtual const char* msg_name() override { return "ZUC_JOG_MULTI_JOINT"; }
    RobJointVal pos;
    double vel;
};

class ZUC_JOG_MOVJ : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_JOG_MOVJ)

    int mvOpt;  // bit0~3: 0 for abs, 1 for rel; bit4~7: 1->jog mode(rapidrate disabled), 0 for auto mode(rapidrate enabled)
    RobJointVal pos_cmd;
    double vel;
    double acc;
    double tol;
    int end_cond[3];
    int executing_line_id;
    int is_cpos;
};

class ZUC_JOG_MOVL : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_JOG_MOVL)
    int mvOpt;  // bit0~3: 0 for abs, 1 for rel; bit4~7: 1->jog mode(rapidrate disabled), 0 for auto mode(rapidrate enabled)
    double pos_cmd[ZUCMOT_MAX_AXIS];
    double vel;
    double acc;
    double tol;
    int end_cond[3];
    int executing_line_id;
    double ori_vel, ori_acc;
};

class ZUC_JOG_MOVC : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_JOG_MOVC)
    int mvOpt;  // bit0~3: 0 for abs, 1 for rel; bit4~7: 1->jog mode(rapidrate disabled), 0 for auto mode(rapidrate enabled)
    double pos_mid[ZUCMOT_MAX_AXIS];
    double pos_end[ZUCMOT_MAX_AXIS];
    double vel;
    double acc;
    double tol;
    int circle_cnt;
    int end_cond[3];
    int executing_line_id;
    int circlemode;  // 0-standard 1-midposeref 2-fixed
    // int use_major_arc;  // 0-using minor arc, 1-using major arc
};

class ZUC_JOG_STOP : public ZUC_JOG_CMD_MSG
{
public:
    ZUC_JOG_STOP() : ZUC_JOG_CMD_MSG(ZUC_JOG_STOP_TYPE, sizeof(ZUC_JOG_STOP)){};

    // For internal NML/CMS use only.
    void update_param(CMS* cms);

    int jjogmode;  // 1==> joint jog, 0==> axis jog, 2 ==> axis jog in tool coordinate
};

class ZUC_SET_CUSTOM_POSE : public ZUC_ROB_CMDITF
{
public:
    ZUC_SET_CUSTOM_POSE() : ZUC_ROB_CMDITF(ZUC_SET_CUSTOM_POSE_TYPE, sizeof(ZUC_SET_CUSTOM_POSE)){};

    int type;  // 0-> shrunken pose, 1->streched pose, 2->initial pose, 3->poseTol
    RobJointVal jpos;
    double poseTol;
    void update_param(CMS* cms);
};

class ZUC_SET_SERVOJ_PARAMETER : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_SET_SERVOJ_PARAMETER)

    int servo_filter_type;
    int mmf_buf_size;
    double jerk;
    double mmf_ka;
    double mmf_kv;
    double mmf_kp;
    double nlf_vp;
    double nlf_ap;
    double nlf_jp;
    double nlf_vr;
    double nlf_ar;
    double nlf_jr;
    double lpf_cutoff_freq;
};

class ZUC_SERVO_DYNAMICS_PARA : public ZUC_ROB_CMDITF
{
public:
    ZUC_SERVO_DYNAMICS_PARA() : ZUC_ROB_CMDITF(ZUC_SERVO_DYNAMICS_PARA_TYPE, sizeof(ZUC_SERVO_DYNAMICS_PARA)) {}

    void update_param(CMS* cms);

    int servo_dynamics_para[ZUCMOT_MAX_JOINTS * 14];
};

class ZUC_SERVO_FRICTION : public ZUC_ROB_CMDITF
{
public:
    ZUC_SERVO_FRICTION() : ZUC_ROB_CMDITF(ZUC_SERVO_FRICTION_TYPE, sizeof(ZUC_SERVO_FRICTION)) {}

    void update_param(CMS* cms);

    double friction_para[ZUCMOT_MAX_JOINTS * 14];
    short jnt_id;
};

class ZUC_COLLISION_SENSITIVITY : public ZUC_ROB_CMDITF
{
public:
    ZUC_COLLISION_SENSITIVITY() : ZUC_ROB_CMDITF(ZUC_COLLISION_SENSITIVITY_TYPE, sizeof(ZUC_COLLISION_SENSITIVITY)) {}

    void update_param(CMS* cms);

    int joint_sensitivity[ZUCMOT_MAX_JOINTS];
};
class ZUC_BACKDRIVE_ON : public ZUC_ROB_CMDITF
{
public:
    ZUC_BACKDRIVE_ON() : ZUC_ROB_CMDITF(ZUC_BACKDRIVE_ON_TYPE, sizeof(ZUC_BACKDRIVE_ON)) {}

    void update_param(CMS* cms);

    int enable;
};

/////////////////////////////////////////////////////////

class ZUC_TRAJ_SET_MAX_VELOCITY : public ZUC_ROB_CMDITF
{
public:
    ZUC_TRAJ_SET_MAX_VELOCITY() : ZUC_ROB_CMDITF(ZUC_TRAJ_SET_MAX_VELOCITY_TYPE, sizeof(ZUC_TRAJ_SET_MAX_VELOCITY)){};

    // For internal NML/CMS use only.
    void update_param(CMS* cms);

    double velocity;
};

class ZUC_RETURN_TO_INITIAL_POSITION : public ZUC_ROB_CMDITF
{
public:
    ZUC_RETURN_TO_INITIAL_POSITION() : ZUC_ROB_CMDITF(ZUC_RETURN_TO_INITIAL_POSITION_TYPE, sizeof(ZUC_RETURN_TO_INITIAL_POSITION)){};
    // For internal NML/CMS use only.
    void update_param(CMS* cms) {}
};

class ZUC_TRAJ_SET_SCALE : public ZUC_ROB_CMDITF
{
public:
    ZUC_TRAJ_SET_SCALE() : ZUC_ROB_CMDITF(ZUC_TRAJ_SET_SCALE_TYPE, sizeof(ZUC_TRAJ_SET_SCALE)){};

    // For internal NML/CMS use only.
    void update_param(CMS* cms);

    double scale;
};

class ZUC_TRAJ_SET_RAPID_SCALE : public ZUC_ROB_CMDITF
{
public:
    ZUC_TRAJ_SET_RAPID_SCALE() : ZUC_ROB_CMDITF(ZUC_TRAJ_SET_RAPID_SCALE_TYPE, sizeof(ZUC_TRAJ_SET_RAPID_SCALE)){};

    // For internal NML/CMS use only.
    void update_param(CMS* cms);

    double scale;
};

class ZUC_TRAJ_LINEAR_MOVE : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_TRAJ_LINEAR_MOVE)

    int type;
    ZucPose end;  // end point
    double vel, ini_maxvel, acc, jerk;
    double ori_vel, ori_acc;
    int feed_mode;
    int indexrotary;
    int using_abc;
    int di_type;
    int di_index;
    int di_state;
    int planner_type;  // 0:T plan, 1:S plan, 2: using toppra for planning

    int tool_id;       //指令运行时使用的工具id
    int usrframe_id;   //指令运行时使用的用户坐标系id
    int payload_id;    //指令运行时使用的负载id
    int blend_mode;    //转接模式，0-不转接，1-转接
    double blend_tol;  //转接半径
};

class ZUC_TRAJ_CIRCULAR_MOVE : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_TRAJ_CIRCULAR_MOVE)

    ZucPose end;
    PM_CARTESIAN center;
    PM_CARTESIAN normal;
    int turn;
    int type;
    double vel, ini_maxvel, acc, jerk;
    int feed_mode;

    int tool_id;       //指令运行时使用的工具id
    int usrframe_id;   //指令运行时使用的用户坐标系id
    int payload_id;    //指令运行时使用的负载id
    int blend_mode;    //转接模式，0-不转接，1-转接
    double blend_tol;  //转接半径
};

class ZUC_TRAJ_MOVC : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_TRAJ_MOVC)

    ZucPose midPoint;
    ZucPose endPoint;
    double vel, ini_maxvel, acc, jerk;
    int using_abc;
    double circle_count;
    int di_type;
    int di_index;
    int di_state;
    int planner_type;   // 0:T plan, 1:S plan, 2: using toppra for planning
    int circlemode;     // 0-standard 1-midposeref 2-fixed
    int use_major_arc;  // 0-using minor arc, 1-using major arc

    int tool_id;       //指令运行时使用的工具id
    int usrframe_id;   //指令运行时使用的用户坐标系id
    int payload_id;    //指令运行时使用的负载id
    int blend_mode;    //转接模式，0-不转接，1-转接
    double blend_tol;  //转接半径
};

class ZUC_TRAJ_JOINT_MOVE : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_TRAJ_JOINT_MOVE)

    int type;
    ZucPose end;  // end point
    double vel, ini_maxvel, acc, jerk;
    int jointNum;
    int feed_mode;
    int indexrotary;
    int di_type;
    int di_index;
    int di_state;
    int planner_type;  // 0:T plan, 1:S plan, 2: using toppra for planning

    int blend_mode;    //转接模式，0-不转接，1-转接
    double blend_tol;  //转接半径
};

class ZUC_TRAJ_MOVS : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_TRAJ_MOVS)
    int type;
    double movs_a0[ZUCMOT_MAX_JOINTS];
    double movs_a1[ZUCMOT_MAX_JOINTS];
    double movs_a2[ZUCMOT_MAX_JOINTS];
    double movs_a3[ZUCMOT_MAX_JOINTS];
    double movs_time;
};

DEC_CMD_PN(ZUC_SET_USER_ID, int id);
DEC_CMD_PN(ZUC_SET_TOOL_ID, int id);

class ZUC_CONVEYOR_ENABLE : public ZUC_ROB_CMDITF
{
public:
    DEC_CMD_HEADER(ZUC_CONVEYOR_ENABLE)

    int convyr_type;  //1 ---> line; 2 ---> circle
    double x;
    double y;
    double z;
    double p1x;
    double p1y;
    double p1z;
    double p2x;
    double p2y;
    double p2z;
    double p3x;
    double p3y;
    double p3z;
    double pulseEquivalent;
    int rotate_tool;
    double maxDistance;
};
DEC_CMD_P0(ZUC_CONVEYOR_DISABLE);
DEC_CMD_PN(ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET, ZucPose robotToolOffset);
DEC_CMD_PN(ZUC_TRAJ_SET_ROBOT_USER_FRAME, ZucPose robotUserFrame);
DEC_CMD_PN(ZUC_TRAJ_CALIB_DHPARAM, DHParam dhParamError);
DEC_CMD_PN(ZUC_TRAJ_SET_HOME, ZucPose home);
DEC_CMD_PN(ZUC_TRAJ_SET_TELEOP_ENABLE, int enable);
DEC_CMD_PN(ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE, int enable);
DEC_CMD_PN(ZUC_TRAJ_SET_SERVOJOP_ENABLE, int enable);
DEC_CMD_PN(ZUC_TRAJ_SET_ADMITOP_ENABLE, int enable);
DEC_CMD_PN(ZUC_SET_PLANNER_JERK, double joint_jerk; double carte_jerk);
DEC_CMD_PN(ZUC_TRAJ_SET_COMPLIANCE_ENABLE, int compliantType; int compliantEnable);
DEC_CMD_PN(ZUC_TRAJ_SET_ADMITTANCE_FRAME, int ftFrame);
DEC_CMD_PN(ZUC_TRAJ_DISABLE_FORCE_CONTROL, int disableForceCtrl);
DEC_CMD_PN(ZUC_TRAJ_SET_ADMITTANCE_ENABLE, int enable);
DEC_CMD_PN(ZUC_TRAJ_SET_ADMITTANCE_OPTION, int admit_axis_opt[CART_DIM]);
DEC_CMD_PN(ZUC_TRAJ_SET_ADMITTANCE_CONFIG, int axis; int now; FtConfig ftConfig);
DEC_CMD_PN(ZUC_TRAJ_SET_END_FORCE_CONDITION, int axis; EndForceCond endForceCond);
DEC_CMD_PN(ZUC_TRAJ_RIGID_TAP, ZucPose pos; double vel; double ini_maxvel; double acc);

/************************  TOPPRA  ********************************/
DEC_CMD_P0(ZUC_TOPPRA_FINISH_LEFT_TC);
DEC_CMD_P0(ZUC_TOPPRA_FIRST_COMMAND);

/****************************  7 dof  *************************************/
DEC_CMD_PN(Robot_7_DOF_INV_INFORM, int inv_flag; double arm_angle);
DEC_CMD_PN(Robot_SET_7_DOF_KINE_PARAM_ALPHA_K, double k; double alpha);

DEC_CMD_PN(ZUC_SET_DRAG_ENABLE, int enable);

DEC_CMD_PN(ZUC_SET_ROBOT_PAYLOAD, int id);

DEC_CMD_PN(ZUC_SERVO_HOME, int joint);

DEC_CMD_PN(ZUC_TORQSENSOR_SETMODE, int mode);

DEC_CMD_PN(ZUC_TORQSENSOR_SETBRAND, int torqsensorBrand);

DEC_CMD_PN(ZUC_TORQUE_SENSOR_SOFT_LIMIT, double Fx; double Fy; double Fz; double Mx; double My; double Mz);

DEC_CMD_PN(ZUC_ADMITTANCE_DRAG_DEAD_ZONE, double Fx; double Fy; double Fz; double Mx; double My; double Mz);

DEC_CMD_PN(ZUC_TORQUE_SENSOR_FILTER, double torqueSensorFilter;);

DEC_CMD_PN(ZUC_MOTION_SET_HOME_VEL,
           char setFlag;  // 1: set move velocity
           double homeVel);

DEC_CMD_PN(ZUC_SET_SERVO_PARAM, ServoParam servoParam);

DEC_CMD_P0(ZUC_CLEAR_ERROR);

DEC_CMD_P0(ZUC_MOTION_ABORT);

DEC_CMD_PN(ZUC_SET_BASE_OFFSET, double base_offset[6]);

DEC_CMD_PN(ZUC_SET_SIM_ROBOT, int robotModel);
